/**
 * 四轮麦轮底盘
 *
 * 以码盘正方向为Y轴正方向，右侧为X轴正方向
 * -----右前ID为1----- *
 * -----左前ID为2----- *
 * -----左后ID为3----- *
 * -----右后ID为4----- *
 */

#ifndef CHASSIS_H
#define CHASSIS_H

#include "BSP.h"
#include "Controller.h"
#include "Global.h"
#include "DJ_Driver.h"
#include "Action.h"

#define RADIUS 152.0f                           // 轮子直径
#define LENGTH 130.0f                           // 底盘半长
#define WIDTH 188.0f                            // 底盘半宽
#define LEGNTH_WIDTH LENGTH + WIDTH             // 半长半宽之和
#define RR 228.57f                              // 轮子到中心的距离
#define DECRATIO 19                             // 电机减速比
#define OFFSET 116.0f                           // 码盘偏移
#define COEFFICIENT 60 * DECRATIO / PI / RADIUS // 单位系数

typedef enum
{
    VELOCITY_MODE,
    POSITION_MODE,
} chassis_ctrl_e;

struct chassis_t
{
    /********************** 电机 ************************/
    DJ_Motor_t ChassisMotors[4];

    float Motors_Speed[4]; // 电机速度

    /********************** 底盘参数 ************************/
    chassis_ctrl_e ctrlMode; // 控制模式

    float setVx; // 底盘设置x速度
    float setVy; // 底盘设置y速度
    float setVw; // 底盘设置角速度

    float setPx;  // 底盘设置x坐标, 单位mm
    float setPy;  // 底盘设置y坐标, 单位mm
    float setYaw; // 偏航角, 绕z轴, 单位rad

    TDPID_t PID_Angle; // 角度pid
    TDPID_t PID_X;     // x坐标pid
    TDPID_t PID_Y;     // y坐标pid
} chassis;

void Chassis_Init(void);
void Chassis_Run(void);
void Chassis_SetVel(float Vx, float Vy, float Vw);
void Chassis_SetPos(float Px, float Py, float Yaw);

#endif
